/*
    Copyright 2004-2005 (c) by Aitor Viana Sanchez,
    Space Research Group,
    University of Alcala.

    For further information, please visit http://srg.aut.uah.es

    This software is provided under the terms of the GNU General Public v2
    Licence. A full copy of the GNU GPL is provided in the file COPYING
    found in the development root of ERCOS-RT.
*/
/* use global definitions */
#include <public/config.h>
/* use the posix interface */
#include <public/posix.h>
/* others */
#include <edroombp_swr/posix/linux_2_6/edroombp.h>
#include <edroombp_swr/posix/linux_2_6/timespec.h>
#include <public/debug_primitives.h>
#include <public/string.h>
#include <time.h>

#ifdef CONFIG_OS_LINUX
int clock_gettime(int clk_id, struct timespec * time)
{
    struct timeval current_time;
    int ret;

    if ((ret = gettimeofday(&current_time, NULL)) != 0)
    {
        return ret;
    }

    time->tv_sec = current_time.tv_sec;
    time->tv_nsec = current_time.tv_usec * 1000;

    return 0;
}
#endif

static pthread_key_t key;

//******************************************************
//****************  Pr_Kernel **************************
//******************************************************
static struct timespec KernelStartTime;

Pr_Kernel::Pr_Kernel()
{
    pthread_key_create(&key, (void(*)(void*))NULL);

    clock_gettime(CLOCK_REALTIME, &KernelStartTime);

}

void Pr_Kernel::Start()
{

#ifdef _EDROOM_IS_DEBUG_
	Pr_SemaphoreBin StartSem("StartSem", 0);
#else
	Pr_SemaphoreBin StartSem(0);
#endif

	setpriority(PRIO_PROCESS, 0, EDROOMprioIDLE);
	/* freeze */
	StartSem.Wait();
}

void Pr_Kernel::Panic (const char * msg)
{
	PANIC (__FILE__, __LINE__, msg);
	while (1);
}

//******************************************************
//****************  Pr_Task ****************************
//******************************************************


//****************  CONSTRUCTORS ***********************

#ifdef _EDROOM_IS_DEBUG_

Pr_Task::Pr_Task(Pr_TaskRV_t (*_taskCode)(Pr_TaskP_t), /*  Task IP */
                const char *_p_name,              /*  Task name   */
                TEDROOMPriority _priority,     /*  task priority   */
                unsigned _stackSize):name(_p_name),        /*  stack size  */
                                semSend("TaskSem Send",0), semReceive("TaskSem recv",0)
{

#else

Pr_Task::Pr_Task(Pr_TaskRV_t (*_taskCode)(Pr_TaskP_t), /*  Task IP */
                TEDROOMPriority _priority,     /*  task priority   */
                unsigned _stackSize):        /*  stack size  */
                                semSend(0), semReceive(0)
{

#endif

    /*  Stablish the task instruction pointer   */
    this->TaskIP =  _taskCode;

    struct sched_param param;

    param.sched_priority = _priority;

    /*  Init the task priorities    */
    priorityMsg = priorityTmp = _priority;

    /*  Init the default attr thread values */
    pthread_attr_init(&attr);

    /*  Stablish the thread stack size  */
    pthread_attr_setstacksize(&attr, _stackSize);

    /*  Init the attr priority with the param.sched_priority value  */
    pthread_attr_setschedparam(&attr, &param);

    /*  Create the thread   */
//    pthread_create(&taskID, &attr, TaskIP, NULL);
    pthread_create(&taskID, &attr, TaskFunction, (void *)this);

}

Pr_TaskRV_t *Pr_Task::TaskFunction(Pr_TaskP_t *arg)
{
    Pr_Task *mythis = (Pr_Task*)arg;

    pthread_setspecific(key, (const void *)mythis);

    mythis->TaskIP();
    return NULL;
}

void Pr_Task::SetPriority(TEDROOMPriority _priority)
{
    priorityMsg = _priority;

    if (priorityTmp != priorityMsg) {
        priorityTmp = priorityMsg;
        ChangePriority(_priority);
    }
}

void Pr_Task::SetMaxPrioTmp(TEDROOMPriority _priority)
{
    /*
     * We only change the priority if the new priority is greater than
     * (less value) the priorityTmp value
     */
    if (_priority < priorityTmp) {
        priorityTmp = _priority;
        ChangePriority(_priority);
    }
}

void Pr_Task::SetMaxPrioMsg(TEDROOMPriority _priority)
{
    if (_priority < priorityMsg) {
        priorityMsg = _priority;
        if (priorityMsg < priorityTmp) {
            priorityTmp = priorityMsg;
            ChangePriority(_priority);
        }
    }
}

void Pr_Task::RestorePrioMsg(void)
{

    if (priorityTmp != priorityMsg) {
        priorityTmp = priorityMsg;

        ChangePriority(priorityMsg);
    }
}

TEDROOMPriority Pr_Task::GetTaskPrio()
{
    struct sched_param param;
    int policy;

    pthread_getschedparam(taskID, &policy, &param);

    return (TEDROOMPriority)param.sched_priority;
}

void Pr_Task::ChangePriority(TEDROOMPriority _priority)
{
    struct sched_param param;
    int policy;

    /*  We do not care about the task sched parameters */
    pthread_getschedparam(taskID, &policy, &param);

    param.sched_priority = _priority;

    /*  Sets the task priority  */
    pthread_setschedparam(taskID, policy, &param);
}

//******************************************************
//****************  Pr_Time ****************************
//******************************************************

//****************  CONSTRUCTORS ***********************

Pr_Time::Pr_Time()
{

    time.tv_sec = 0;
    time.tv_nsec = 0;
}

Pr_Time::Pr_Time(const Pr_Time &_time)
{
    time.tv_nsec = _time.time.tv_nsec;
    time.tv_sec = _time.time.tv_sec;
}

Pr_Time::Pr_Time(float _seconds)
{
    float secs = _seconds;

    time.tv_sec = time.tv_nsec = 0;

    if(_seconds) {
        time.tv_sec = (time_t)secs;
        secs -= time.tv_sec;
        time.tv_nsec = (unsigned long)(secs * 1000000000);

    }

}

Pr_Time::Pr_Time(struct timespec _time)
{
    unsigned long nsec;
    time_t sec;

    sec = (time_t)(_time.tv_nsec / 1000000000);

    time.tv_sec = _time.tv_sec + sec;
    nsec = _time.tv_nsec - (sec * 1000000000);
    time.tv_nsec = nsec;
}


Pr_Time::Pr_Time(Pr_Time::unit _units, uint32_t _measure)
{
    time.tv_sec = time.tv_nsec = 0;

    if (_measure) {
        switch (_units) {
            case ticks:
                timespec_from_us (&time, _measure);
                break;
            case micros:
                time.tv_nsec = _measure * 1000;
                timespec_normalize (&time);
                break;
            case millis:
                time.tv_nsec = _measure * 1000000;
                timespec_normalize (&time);
                break;
            case secns:
                time.tv_sec = _measure;
                break;
            case mints:
                time.tv_sec = _measure * 60;
                break;
        }
    }
}

//**************** MODIFIYING METHODS **********************

void Pr_Time::GetTime(void)
{
    struct timespec timeval;

    if (clock_gettime(CLOCK_REALTIME, &timeval) == 0)
    {
    	timespec_sub(&timeval,&KernelStartTime);
    	time.tv_nsec = timeval.tv_nsec;
        time.tv_sec = timeval.tv_sec;
    }

}

EDROOMClockTicksType Pr_Time::GetTicks()
{
    struct timespec current_time;
    EDROOMClockTicksType ticks = 0;

    if (clock_gettime(CLOCK_REALTIME, &current_time) == 0)
    {

        timespec_sub(&current_time,&KernelStartTime);
        _POSIX_timespec_to_ticks(&current_time, ticks);
    }

    return ticks;
}

//**********   OPERATORS OVERLOAD  *********************

Pr_Time& Pr_Time::operator+=(const Pr_Time &_time)
{

    timespec_add (&time, &(_time.time));

    return *this;
}

Pr_Time& Pr_Time::operator-=(const Pr_Time &_time)
{
    if (_time.time.tv_sec <= time.tv_sec)
    {
        time.tv_sec -= _time.time.tv_sec;
        if (_time.time.tv_nsec > time.tv_nsec)
        {
            if (time.tv_sec)
            {
                time.tv_sec--;
                time.tv_nsec += 1000000000 - _time.time.tv_nsec;
            }
            else
                time.tv_sec = time.tv_nsec = 0;
        }
        else
            time.tv_nsec -= _time.time.tv_nsec;
    }
    else
        time.tv_sec = time.tv_nsec = 0;

    //  _POSIX_timespec_sub(&time, &(_time.time));

    return *this;
}

Pr_Time& Pr_Time::operator=(const Pr_Time &_time)
{
    time.tv_nsec = _time.time.tv_nsec;
    time.tv_sec = _time.time.tv_sec;

    return *this;
}

int Pr_Time::operator==(const Pr_Time &_time)
{
    return timespec_eq (&time, &(_time.time));

}

int Pr_Time::operator!=(const Pr_Time &_time)
{
    return ! timespec_eq (&time, &(_time.time));

}

int Pr_Time::operator>(const Pr_Time &_time)
{
    return timespec_gt (&time, &(_time.time));
}

int Pr_Time::operator<(const Pr_Time &_time)
{
    return timespec_lt (&time, &(_time.time));
}

int Pr_Time::operator>=(const Pr_Time &_time)
{
    return timespec_ge (&time, &(_time.time));
}

int Pr_Time::operator<=(const Pr_Time &_time)
{
    return timespec_le (&time, &(_time.time));
}

//*******   CONVERSION METHODS TO STANDARD UNITS   ******

float Pr_Time::Mints() const
{
    clock_t nticks;

    nticks = time.tv_sec * CLICKS_PER_SEC;

    if (time.tv_nsec > 1000)
        nticks = nticks + (time.tv_nsec / 1000) * CLICKS_PER_USEC;

    return (float)(uint32_t)nticks / (CLICKS_PER_SEC * 60);
}

float Pr_Time::Secns() const
{
    float aux;

    aux = time.tv_sec + time.tv_nsec * 0.000000001;

    return (float)(aux);
}

float Pr_Time::Millis() const
{
    float aux;

    aux = time.tv_sec + (float)time.tv_nsec / 1000000000.0;

    return(aux * 1000);

}

float Pr_Time::Micros() const
{
    float aux;

    aux = time.tv_sec + (float)time.tv_nsec / 1000000000;


    return (float)(aux * 1000000);
}
#ifdef CONFIG_EDROOMBP_POSIX_TESTS

float Pr_Time::Nanos() const
{
    float aux;

    aux = time.tv_sec + (float)time.tv_nsec / 1000000000;


    return (float)(aux * 1000000000);
}
#endif

EDROOMClockTicksType Pr_Time::Ticks() const
{

	 EDROOMClockTicksType ticks = 0;


	_POSIX_timespec_to_ticks(&time, ticks);

    return ticks;
}

void Pr_DelayIn(const Pr_Time &_interval)
{
    struct timespec time = _interval.time;

    nanosleep(&time, (struct timespec*)NULL);

}

void Pr_DelayAt( const Pr_Time &_abs_time_since_reset)
{
    //int ret;

    struct timespec interval= _abs_time_since_reset.time;
    struct timespec current_time;

    if (clock_gettime(CLOCK_REALTIME, &current_time) == 0)
    {

            timespec_sub(&current_time,&KernelStartTime);

            if(timespec_ge (&interval,&current_time)){

            	timespec_sub(&interval,&current_time);

            	nanosleep(&interval, (struct timespec*)NULL);

            }
    }



   // ret = clock_nanosleep (CLOCK_REALTIME, TIMER_ABSTIME, &abs_time, (struct timespec *) 0);
}

//********************************************************
//********************  Pr_Semaphore  ********************
//********************************************************

Pr_Semaphore::Pr_Semaphore(unsigned int _value)
{
}


//********************************************************
//********************  Pr_SemaphoreBin  *****************
//********************************************************


#ifdef _EDROOM_IS_DEBUG_
Pr_SemaphoreBin::Pr_SemaphoreBin(const char *p_name, uint32_t _value) :
 Pr_Semaphore(_value), name(p_name)
{
    sem_init(&sem, 0, _value);
}

#else

Pr_SemaphoreBin::Pr_SemaphoreBin(uint32_t _value) : Pr_Semaphore(_value)
{
    sem_init(&sem, 0, _value);
}

#endif

void Pr_SemaphoreBin::Signal()
{
    sem_post(&sem);
}

void Pr_SemaphoreBin::Wait()
{
    sem_wait(&sem);
}

int Pr_SemaphoreBin::WaitCond()
{
    int retval = 0;

    if (sem_trywait(&sem) == 0)
        retval = 1;

    return retval;          /*  OK, return 1    */
}

bool Pr_SemaphoreBin::WaitTimed(const Pr_Time &_waittime)
{
    bool retval;
    struct timespec time;
    struct timespec current;

    time.tv_sec = _waittime.time.tv_sec;
    time.tv_nsec = _waittime.time.tv_nsec;

    if (clock_gettime(CLOCK_REALTIME, &current) == 0)
    {
    	timespec_add(&time, &current);

    }
    retval = sem_timedwait (&sem, &time);

    return (retval == 0);
}

//********************************************************
//********************  Pr_SemaphoreRec  *****************
//********************************************************

#ifdef _EDROOM_IS_DEBUG_
Pr_SemaphoreRec::Pr_SemaphoreRec(const char *p_name) : name(p_name)
#else
Pr_SemaphoreRec::Pr_SemaphoreRec()
#endif
{
    pthread_mutexattr_init(&attr);
    pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_NORMAL);
    pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT);
    pthread_mutex_init(&mutex, &attr);
}

#ifdef _EDROOM_IS_DEBUG_
Pr_SemaphoreRec::Pr_SemaphoreRec(const char *p_name, int32_t prioceiling) : name(p_name)
#else
Pr_SemaphoreRec::Pr_SemaphoreRec(int32_t prioceiling)
#endif
{
    int priority_ceiling;

    pthread_mutexattr_init(&attr);
    pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_NORMAL);
    pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_PROTECT);

    /*
     * The priority in pthread_mutex with priority ceiling protocol
     * varies from 0 to 127 means 127 the higher priority. In EDROOM
     * the 0 priority value is the higher priority so we need to make a
     * change
     */
    priority_ceiling = EDROOM_PRIO_MINIMUM - prioceiling;
    priority_ceiling = (priority_ceiling < 0) ? 0 : priority_ceiling;

    pthread_mutexattr_setprioceiling (&attr, priority_ceiling);
    pthread_mutex_init(&mutex, &attr);
}

void Pr_SemaphoreRec::Signal()
{
    pthread_mutex_unlock(&mutex);
}

void Pr_SemaphoreRec::Wait()
{
    pthread_mutex_lock(&mutex);
}

int Pr_SemaphoreRec::WaitCond()
{
    if (pthread_mutex_trylock(&mutex) == 0)
        return 1;
    else
        return 0;
}


void Pr_Send(Pr_Task &_task, void *_p_data)
{
    _task.p_data = _p_data;
    _task.semReceive.Signal();
    _task.semSend.Wait();
}

void Pr_Receive(void * _p_data,unsigned _datalength)
{
    Pr_Task *receiver;

    receiver = (Pr_Task*)pthread_getspecific(key);

    receiver->semReceive.Wait();

    uint8_t *p_dataAux, *p_dataAuxReceiver;

    p_dataAux = (uint8_t *)_p_data;
    p_dataAuxReceiver = (uint8_t *)receiver->p_data;

    for (unsigned int i = 0; i < _datalength; ++i)
        *(p_dataAux + i) = *(p_dataAuxReceiver + i);


    receiver->semSend.Signal();
}

//********************************************************
//********************  Pr_IRQManager ********************
//********************************************************

void Pr_IRQManager::DisableIRQ(uint32_t IRQVectorNumber)
{
//	intr_lock (IRQVectorNumber);
}

void Pr_IRQManager::EnableIRQ(uint32_t IRQVectorNumber)
{
//	intr_unlock (IRQVectorNumber);
}

void Pr_IRQManager::DisableAllIRQs(void)
{
	intr_disable ();
}

void Pr_IRQManager::ApplyCurrentIRQMask(void)
{
	intr_enable ();
}

void Pr_IRQManager::InstallIRQHandler(Pr_IRQHandler handler,
		uint8_t IRQLevel, uint8_t IRQVectorNumber )
{

	intr_capture((void (*) (int)) handler, IRQVectorNumber);
}

void Pr_IRQManager::DeinstallIRQHandler(unsigned char IRQLevel,
		unsigned char IRQVectorNumber)
{

	intr_restore (IRQVectorNumber);
}

//********************************************************
//********************  Pr_IRQEvent ********************
//********************************************************

#ifdef _EDROOM_IS_DEBUG_

Pr_IRQEvent::Pr_IRQEvent(const char * pname,unsigned char IRQ) :
                        eventSem("Event Semaphore", 0), // init the semaphore
                        name(pname)                     // the Event name
{
}

#else

Pr_IRQEvent::Pr_IRQEvent(unsigned char IRQ) :
                        eventSem(0)    // init the semaphore
{}

#endif

void Pr_IRQEvent::Signal()
{
    eventSem.Signal();
}

void Pr_IRQEvent::Wait()
{
    eventSem.Wait();
}

bool Pr_IRQEvent::WaitTimed(Pr_Time _time)
{
    return eventSem.WaitTimed(_time);
}

bool Pr_IRQEvent::WaitCond()
{
    return eventSem.WaitCond();
}

//********************************************************
//********************  Pr_Trace  ************************
//********************************************************

#ifdef CONFIG_EDROOMBP_SWR_TRACE_METHODS
Pr_Trace Pr_deb ("EDROOM-trace", 2);
Pr_Trace Pr_err ("EDROOM-error", 3);

Pr_Trace::Pr_Trace (void) {
	/* log trace events at level 2 */
	open (NULL, 2);
}

Pr_Trace::Pr_Trace (const char * str) {
	open (str, 2);
}

Pr_Trace::Pr_Trace (const char * str, unsigned level) {
	open (str, level);
}

void Pr_Trace::open (const char * str, unsigned level) {
	int ret;
	trace_id_t dummy;

	/* create the trace log */
	ret = posix_trace_create (0, NULL, &dummy);
	ASSERT (ret == 0);

	/* create a new event type */
	ret = posix_trace_eventid_open_with_level (str, &evid, level);
	ASSERT (ret == 0);
}

void Pr_Trace::close (void) {
	/* FIXME -- have we got to call to a posix_trace_close primitive ? */
}

void Pr_Trace::trace (const uint64_t & val) {
	char buff[16];
	unsigned int nr;

	/* FIXME -- printf hasn't got long long conversions */
	nr = sprintf (buff, "%u", (uint32_t) val);
	posix_trace_event (evid, (void *) buff, nr);
}

void Pr_Trace::trace (const uint32_t & val) {
	char buff[16];
	unsigned int nr;

	nr = sprintf (buff, "%u", val);
	posix_trace_event (evid, (void *) buff, nr);
}

void Pr_Trace::trace (const int32_t & val) {
	char buff[16];
	unsigned int nr;

	nr = sprintf (buff, "%d", val);
	posix_trace_event (evid, (void *) buff, nr);
}

void Pr_Trace::trace (const float & val) {
	char buff[16];
	unsigned int nr;

	/* FIXME -- printf hasn't float conversion */
	nr = sprintf (buff, "%d", (int32_t) val);
	posix_trace_event (evid, (void *) buff, nr);
}

Pr_Trace & Pr_Trace::operator << (const char * str) {
	posix_trace_event (evid, (void *) str, strlen (str));
	return *this;
}

Pr_Trace & Pr_Trace::operator << (uint64_t & val) {
	trace (val);
	return *this;
}

Pr_Trace & Pr_Trace::operator << (uint32_t val) {
	trace (val);
	return *this;
}

Pr_Trace & Pr_Trace::operator << (int32_t & val) {
	trace (val);
	return *this;
}

Pr_Trace & Pr_Trace::operator << (uint16_t & val) {
	trace ((uint32_t) val);
	return *this;
}

Pr_Trace & Pr_Trace::operator << (uint8_t & val) {
	trace ((uint32_t) val);
	return *this;
}

Pr_Trace & Pr_Trace::operator << (float & val) {
	trace (val);
	return *this;
}
#endif

